/**
 * @file ebex_matrix.h
 *
 * @date Dec 4, 2009
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2009 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#ifndef EBEX_MATRIX_H_
#define EBEX_MATRIX_H_
#include <mkl.h>
#include <mkl_lapacke.h>

#include <memory_util.h>
#include <ebex_math.h>
#include <ebex_vector.h>

void ebex_mat_tridiag_ql_4x4(ebex_mat4x4_t *m_eigenvectors, ebex_mat4x4_t *m_eigenvalues, ebex_mat4x4_t *m_subdiag);
void ebex_mat_tridiag_reduce_4x4(ebex_mat4x4_t *m_matrix, ebex_mat4x4_t *m_diagonal, ebex_mat4x4_t *m_offdiagonal);
int ebex_3mat_sqrt( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat);
void ebex_3mat_from_euler(ebex_mat3x3_t *m_result, const ebex_3vec_t *m_src, e_rotation_order m_order);
void ebex_mat_eigenv_4x4(ebex_mat4x4_t *m_eigenvectors, ebex_quat_t *m_output, ebex_mat4x4_t *m_input);

static inline void ebex_mat_eigenv(ebex_mat4x4_t *m_eigenvectors, ebex_mat4x4_t *m_eigenvalues, ebex_mat4x4_t *m_input)
{
	ebex_mat4x4_t tmp;
	ebex_mat4x4_t eigenvec;

	memcpy(&eigenvec, m_input, sizeof(ebex_mat4x4_t));
	LAPACKE_dsyev(LAPACK_COL_MAJOR, 'V', 'U', 4, eigenvec.element, 4, tmp.element);

	if (m_eigenvalues) memcpy(m_eigenvalues, &tmp, sizeof(ebex_mat4x4_t));
	memcpy(m_eigenvectors, &eigenvec, sizeof(ebex_mat4x4_t));

}

static inline void ebex_3mat_copy( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat )
{
    ebex_3vec_copy( &m_result->col[0], &m_mat->col[0] );
    ebex_3vec_copy( &m_result->col[1], &m_mat->col[1] );
    ebex_3vec_copy( &m_result->col[2], &m_mat->col[2] );
}

static inline void ebex_3mat_from_scalar( ebex_mat3x3_t *m_result, double scalar )
{
    ebex_3vec_make_from_scalar( &m_result->col[0], scalar );
    ebex_3vec_make_from_scalar( &m_result->col[1], scalar );
    ebex_3vec_make_from_scalar( &m_result->col[2], scalar );
}

static inline void ebex_3mat_from_cols( ebex_mat3x3_t *m_result, const ebex_3vec_t *_col0, const ebex_3vec_t *_col1, const ebex_3vec_t *_col2 )
{
    ebex_3vec_copy( &m_result->col[0], _col0 );
    ebex_3vec_copy( &m_result->col[1], _col1 );
    ebex_3vec_copy( &m_result->col[2], _col2 );
}

static inline void ebex_3mat_get_col( ebex_3vec_t *m_result, const ebex_mat3x3_t *m_mat, int col )
{
    ebex_3vec_copy( m_result, (&m_mat->col[0] + col) );
}

static inline void ebex_3mat_get_row( ebex_3vec_t *m_result, const ebex_mat3x3_t *m_mat, int row )
{
    ebex_3vec_make_from_elems( m_result, ebex_3vec_get_elem( &m_mat->col[0], row ), ebex_3vec_get_elem( &m_mat->col[1], row ), ebex_3vec_get_elem( &m_mat->col[2], row ) );
}

static inline void ebex_3mat_set_col( ebex_mat3x3_t *m_result, int col, const ebex_3vec_t *m_vec )
{
    ebex_3vec_copy( (&m_result->col[0] + col), m_vec );
}

static inline void ebex_3mat_set_row( ebex_mat3x3_t *m_result, int row, const ebex_3vec_t *m_vec )
{
    ebex_3vec_set_elem( &m_result->col[0], row, ebex_3vec_get_elem( m_vec, 0 ) );
    ebex_3vec_set_elem( &m_result->col[1], row, ebex_3vec_get_elem( m_vec, 1 ) );
    ebex_3vec_set_elem( &m_result->col[2], row, ebex_3vec_get_elem( m_vec, 2 ) );
}

static inline void ebex_3mat_set_elem( ebex_mat3x3_t *m_result, int col, int row, double val )
{
   m_result->col[col].element[row] = val;
}

static inline double ebex_3mat_get_elem( const ebex_mat3x3_t *m_mat, int col, int row )
{
    return m_mat->col[col].element[row];
}

static inline double ebex_3mat_trace ( ebex_mat3x3_t *m_mat)
{
	return m_mat->col[0].element[0] + m_mat->col[1].element[1] + m_mat->col[2].element[2];
}

static inline void ebex_3mat_transpose( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat )
{
    ebex_mat3x3_t tmp;
    ebex_3vec_make_from_elems( &tmp.col[0], m_mat->col[0].x, m_mat->col[1].x, m_mat->col[2].x );
    ebex_3vec_make_from_elems( &tmp.col[1], m_mat->col[0].y, m_mat->col[1].y, m_mat->col[2].y );
    ebex_3vec_make_from_elems( &tmp.col[2], m_mat->col[0].z, m_mat->col[1].z, m_mat->col[2].z );
    ebex_3mat_copy( m_result, &tmp );
}

static inline void ebex_3mat_inverse( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat )
{
    ebex_3vec_t tmp0, tmp1, tmp2;
    double detinv;
    ebex_3vec_cross( &tmp0, &m_mat->col[1], &m_mat->col[2] );
    ebex_3vec_cross( &tmp1, &m_mat->col[2], &m_mat->col[0] );
    ebex_3vec_cross( &tmp2, &m_mat->col[0], &m_mat->col[1] );
    detinv = ( 1.0 / ebex_3vec_dot( &m_mat->col[2], &tmp2 ) );
    ebex_3vec_make_from_elems( &m_result->col[0], ( tmp0.x * detinv ), ( tmp1.x * detinv ), ( tmp2.x * detinv ) );
    ebex_3vec_make_from_elems( &m_result->col[1], ( tmp0.y * detinv ), ( tmp1.y * detinv ), ( tmp2.y * detinv ) );
    ebex_3vec_make_from_elems( &m_result->col[2], ( tmp0.z * detinv ), ( tmp1.z * detinv ), ( tmp2.z * detinv ) );
}

static inline double ebex_3mat_determinant( const ebex_mat3x3_t *m_mat )
{
    ebex_3vec_t tmp_v0;
    ebex_3vec_cross( &tmp_v0, &m_mat->col[0], &m_mat->col[1] );
    return ebex_3vec_dot( &m_mat->col[2], &tmp_v0 );
}

static inline void ebex_3mat_add( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat0, const ebex_mat3x3_t *m_mat1 )
{
    ebex_3vec_add( &m_result->col[0], &m_mat0->col[0], &m_mat1->col[0] );
    ebex_3vec_add( &m_result->col[1], &m_mat0->col[1], &m_mat1->col[1] );
    ebex_3vec_add( &m_result->col[2], &m_mat0->col[2], &m_mat1->col[2] );
}

static inline void ebex_3mat_sub( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat0, const ebex_mat3x3_t *m_mat1 )
{
    ebex_3vec_sub( &m_result->col[0], &m_mat0->col[0], &m_mat1->col[0] );
    ebex_3vec_sub( &m_result->col[1], &m_mat0->col[1], &m_mat1->col[1] );
    ebex_3vec_sub( &m_result->col[2], &m_mat0->col[2], &m_mat1->col[2] );
}

static inline void ebex_3mat_neg( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat )
{
    ebex_3vec_neg( &m_result->col[0], &m_mat->col[0] );
    ebex_3vec_neg( &m_result->col[1], &m_mat->col[1] );
    ebex_3vec_neg( &m_result->col[2], &m_mat->col[2] );
}

static inline void ebex_3mat_abs_per_elem( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat )
{
    ebex_3vec_abs_per_elem( &m_result->col[0], &m_mat->col[0] );
    ebex_3vec_abs_per_elem( &m_result->col[1], &m_mat->col[1] );
    ebex_3vec_abs_per_elem( &m_result->col[2], &m_mat->col[2] );
}

static inline void ebex_3mat_scalar_mul( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat, double scalar )
{
    ebex_3vec_scalar_mul( &m_result->col[0], &m_mat->col[0], scalar );
    ebex_3vec_scalar_mul( &m_result->col[1], &m_mat->col[1], scalar );
    ebex_3vec_scalar_mul( &m_result->col[2], &m_mat->col[2], scalar );
}

static inline void ebex_3mat_mul_v3( ebex_3vec_t *m_result, const ebex_mat3x3_t *m_mat, const ebex_3vec_t *m_vec )
{
    double tmpx, tmpy, tmpz;
    tmpx = ( ( ( m_mat->col[0].x * m_vec->x ) + ( m_mat->col[1].x * m_vec->y ) ) + ( m_mat->col[2].x * m_vec->z ) );
    tmpy = ( ( ( m_mat->col[0].y * m_vec->x ) + ( m_mat->col[1].y * m_vec->y ) ) + ( m_mat->col[2].y * m_vec->z ) );
    tmpz = ( ( ( m_mat->col[0].z * m_vec->x ) + ( m_mat->col[1].z * m_vec->y ) ) + ( m_mat->col[2].z * m_vec->z ) );
    ebex_3vec_make_from_elems( m_result, tmpx, tmpy, tmpz );
}

static inline void ebex_3mat_mul( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat0, const ebex_mat3x3_t *m_mat1 )
{
    ebex_mat3x3_t tmp;
    ebex_3mat_mul_v3( &tmp.col[0], m_mat0, &m_mat1->col[0] );
    ebex_3mat_mul_v3( &tmp.col[1], m_mat0, &m_mat1->col[1] );
    ebex_3mat_mul_v3( &tmp.col[2], m_mat0, &m_mat1->col[2] );
    ebex_3mat_copy( m_result, &tmp );
}

static inline void ebex_3mat_mul_per_elem( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat0, const ebex_mat3x3_t *m_mat1 )
{
    ebex_3vec_mul_per_elem( &m_result->col[0], &m_mat0->col[0], &m_mat1->col[0] );
    ebex_3vec_mul_per_elem( &m_result->col[1], &m_mat0->col[1], &m_mat1->col[1] );
    ebex_3vec_mul_per_elem( &m_result->col[2], &m_mat0->col[2], &m_mat1->col[2] );
}

static inline void ebex_3mat_make_identity( ebex_mat3x3_t *m_result )
{
    ebex_3vec_make_xaxis( &m_result->col[0] );
    ebex_3vec_make_yaxis( &m_result->col[1] );
    ebex_3vec_make_zaxis( &m_result->col[2] );
}

static inline void ebex_3mat_make_rotation_x( ebex_mat3x3_t *m_result, double radians )
{
    double s, c;

	vdSinCos(1, &radians, &s, &c);
    ebex_3vec_make_xaxis( &m_result->col[0] );
    ebex_3vec_make_from_elems( &m_result->col[1], 0.0, c, s );
    ebex_3vec_make_from_elems( &m_result->col[2], 0.0, -s, c );
}

static inline void ebex_3mat_make_rotation_y( ebex_mat3x3_t *m_result, double radians )
{
    double s, c;
    vdSinCos(1, &radians, &s, &c);
    ebex_3vec_make_from_elems( &m_result->col[0], c, 0.0, -s );
    ebex_3vec_make_yaxis( &m_result->col[1] );
    ebex_3vec_make_from_elems( &m_result->col[2], s, 0.0, c );
}

static inline void ebex_3mat_make_rotation_z( ebex_mat3x3_t *m_result, double radians )
{
    double s, c;
    vdSinCos(1, &radians, &s, &c);
    ebex_3vec_make_from_elems( &m_result->col[0], c, s, 0.0 );
    ebex_3vec_make_from_elems( &m_result->col[1], -s, c, 0.0 );
    ebex_3vec_make_zaxis( &m_result->col[2] );
}

static inline void ebex_3mat_from_rotation_axis( ebex_mat3x3_t *m_result, double radians, const ebex_3vec_t *unit_vec )
{
    double s, c;
    vdSinCos(1, &radians, &s, &c);

    double x = unit_vec->x;
    double y = unit_vec->y;
    double z = unit_vec->z;
    double xy = ( x * y );
    double yz = ( y * z );
    double zx = ( z * x );

    double one_minus_c = ( 1.0 - c );
    ebex_3vec_make_from_elems( &m_result->col[0], ( ( ( x * x ) * one_minus_c ) + c ), ( ( xy * one_minus_c ) + ( z * s ) ), ( ( zx * one_minus_c ) - ( y * s ) ) );
    ebex_3vec_make_from_elems( &m_result->col[1], ( ( xy * one_minus_c ) - ( z * s ) ), ( ( ( y * y ) * one_minus_c ) + c ), ( ( yz * one_minus_c ) + ( x * s ) ) );
    ebex_3vec_make_from_elems( &m_result->col[2], ( ( zx * one_minus_c ) + ( y * s ) ), ( ( yz * one_minus_c ) - ( x * s ) ), ( ( ( z * z ) * one_minus_c ) + c ) );
}

static inline void ebex_3mat_from_scale_vec( ebex_mat3x3_t *m_result, const ebex_3vec_t *m_scale )
{
    ebex_3vec_make_from_elems( &m_result->col[0], m_scale->x, 0.0, 0.0 );
    ebex_3vec_make_from_elems( &m_result->col[1], 0.0, m_scale->y, 0.0 );
    ebex_3vec_make_from_elems( &m_result->col[2], 0.0, 0.0, m_scale->z );
}

static inline void ebex_3mat_append_scale( ebex_mat3x3_t *m_result, const ebex_mat3x3_t *m_mat, const ebex_3vec_t *m_scale )
{
    ebex_3vec_scalar_mul( &m_result->col[0], &m_mat->col[0], ebex_3vec_get_x( m_scale ) );
    ebex_3vec_scalar_mul( &m_result->col[1], &m_mat->col[1], ebex_3vec_get_y( m_scale ) );
    ebex_3vec_scalar_mul( &m_result->col[2], &m_mat->col[2], ebex_3vec_get_z( m_scale ) );
}

static inline void ebex_3mat_prepend_scale( ebex_mat3x3_t *m_result, const ebex_3vec_t *m_scale, const ebex_mat3x3_t *m_mat )
{
    ebex_3vec_mul_per_elem( &m_result->col[0], &m_mat->col[0], m_scale );
    ebex_3vec_mul_per_elem( &m_result->col[1], &m_mat->col[1], m_scale );
    ebex_3vec_mul_per_elem( &m_result->col[2], &m_mat->col[2], m_scale );
}

static inline void ebex_3vec_outer( ebex_mat3x3_t *m_result, const ebex_3vec_t *m_vec1, const ebex_3vec_t *m_vec2 )
{
    ebex_3vec_scalar_mul( &m_result->col[0], m_vec1, ebex_3vec_get_x(m_vec2) );
    ebex_3vec_scalar_mul( &m_result->col[1], m_vec1, ebex_3vec_get_y(m_vec2) );
    ebex_3vec_scalar_mul( &m_result->col[2], m_vec1, ebex_3vec_get_z(m_vec2) );
}

static inline void ebex_3vec_row_mul( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec, const ebex_mat3x3_t *m_mat )
{
    double tmpx = ( ( ( m_vec->x * m_mat->col[0].x ) + ( m_vec->y * m_mat->col[0].y ) ) + ( m_vec->z * m_mat->col[0].z ) );
    double tmpy = ( ( ( m_vec->x * m_mat->col[1].x ) + ( m_vec->y * m_mat->col[1].y ) ) + ( m_vec->z * m_mat->col[1].z ) );
    double tmpz = ( ( ( m_vec->x * m_mat->col[2].x ) + ( m_vec->y * m_mat->col[2].y ) ) + ( m_vec->z * m_mat->col[2].z ) );
    ebex_3vec_make_from_elems( m_result, tmpx, tmpy, tmpz );
}

static inline void ebex_3vec_cross_matrix( ebex_mat3x3_t *m_result, const ebex_3vec_t *m_vec )
{
    ebex_3vec_make_from_elems( &m_result->col[0], 0.0, m_vec->z, -m_vec->y );
    ebex_3vec_make_from_elems( &m_result->col[1], -m_vec->z, 0.0, m_vec->x );
    ebex_3vec_make_from_elems( &m_result->col[2], m_vec->y, -m_vec->x, 0.0 );
}

static inline void ebex_3vec_cross_matrix_mul( ebex_mat3x3_t *m_result, const ebex_3vec_t *m_vec, const ebex_mat3x3_t *m_mat )
{
    ebex_3vec_t tmp_v0, tmp_v1, tmp_v2;
    ebex_3vec_cross( &tmp_v0, m_vec, &m_mat->col[0] );
    ebex_3vec_cross( &tmp_v1, m_vec, &m_mat->col[1] );
    ebex_3vec_cross( &tmp_v2, m_vec, &m_mat->col[2] );
    ebex_3mat_from_cols( m_result, &tmp_v0, &tmp_v1, &tmp_v2 );
}



#endif /* EBEX_MATRIX_H_ */
